/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file palletcalculate.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "palletcalculate.h"
#include <Eigen/Dense>

#define IF_DEBUG false

PalletCalculate::PalletCalculate()
{
    setComponentVersion( "PalletCalculate", "2016-12-9", "1.0.0" );

    //---------初始化码垛线程锁--------
    pthread_mutexattr_init(&palletAttr);
    pthread_mutexattr_setprotocol(&palletAttr, PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&palletMutex, &palletAttr);

    number = 0;
}

PalletCalculate::~PalletCalculate()
{
}

// 2017-3-31 彭秋明添加
int PalletCalculate::palletInput(PointData* _pointData, Coordinate* _coordinate)
{
    pointData = _pointData;
    coordinate = _coordinate;

    pthread_mutex_lock(&palletMutex);
    palletAmount = pointData->basePalletList.size();//得到码垛数量
    if (NULL == pointData || NULL == coordinate || 0 == palletAmount)
    {
        return -1;
    }
    palletList.resize(palletAmount);
    palletList = pointData->basePalletList;
    pthread_mutex_unlock(&palletMutex);

    palletVector.resize(palletAmount);
    palletRCL.resize(palletAmount);
    palletPlRCL.resize(palletAmount);
    number = 0;
    return 1;
}

//2017-3-27 彭秋明添加
PalletSave PalletCalculate::returnPalletVector(int palletIndex)
{
    return palletVector[palletIndex];
}

//2017-4-20 彭秋明添加
int PalletCalculate::setPalletFirstNum(int palletIndex)
{
    if( palletIndex >= palletAmount || palletIndex<0 )//超出码垛范围
    {
        return -1;
    }
    else if (palletIndex < 0)
    {
        for (int i = 0; i < palletAmount; i++)
        {
            palletVector[i].firstNum = 0;
            palletVector[i].finishOutput = 0;
            palletVector[i].modifyPlValue = 0;
        }
    }
    else
    {
        palletVector[palletIndex].firstNum = 0;
        palletVector[palletIndex].finishOutput = 0;
        palletVector[palletIndex].modifyPlValue = 0;
    }
    return 1;
}

int PalletCalculate::setPalletPlValue(int palletIndex)
{
    if( palletIndex >= palletAmount )//超出码垛范围
    {
        return -1;
    }
    palletVector[palletIndex].modifyPlValue = 1;
    palletVector[palletIndex].firstNum = 1;
    return 1;
}

int PalletCalculate::readPalletPlValue(int palletIndex, int plRow, int plCol, int plLayer)
{
    if( palletIndex >= palletAmount )//超出码垛范围
    {
        return -1;
    }
    palletPlRCL[palletIndex].row = plRow;
    palletPlRCL[palletIndex].col = plCol;
    palletPlRCL[palletIndex].layer = plLayer;
    return 1;
}

// 2017-3-25 彭秋明添加
void PalletCalculate::frameToPointPro(int palletIndex, int anAmount, int rnAmount, PalletSave &tmpPallet)
{
    if (palletList[palletIndex].stackPointBTM.jointFlag == 1)
    {
        tmpPallet.btmPointPro = coordinate->frameToPointPro_CART(tmpPallet.btmFrame,
                                                                                 palletList[palletIndex].stackPointBTM.ufValue,
                                                                                 palletList[palletIndex].stackPointBTM.utValue);
        //tmpPallet.btmPointPro.configString = tmpPallet.configString;
    }
    else
    {
        coordinate->frameToPointPro_JOINT(tmpPallet.btmFrame, tmpPallet.btmPointPro,
                                          palletList[palletIndex].stackPointBTM.ufValue, palletList[palletIndex].stackPointBTM.utValue);
        //tmpPallet.btmPointPro.configString = tmpPallet.configString;
    }
    tmpPallet.btmPointPro.configString = tmpPallet.configString;

    //tmpPallet.pointAnPointPro.resize(anAmount);
    for (int i = 0; i < anAmount; i++)
    {
        if (palletList[palletIndex].stackPointA_n[i].jointFlag == 1)
        {
            tmpPallet.pointAnPointPro[i] = coordinate->frameToPointPro_CART(tmpPallet.pointAnFrame[i],
                                                                                            palletList[palletIndex].stackPointA_n[i].ufValue,
                                                                                            palletList[palletIndex].stackPointA_n[i].utValue);
            //tmpPallet.pointAnPointPro[i].configString = tmpPallet.configString;
        }
        else
        {
            coordinate->frameToPointPro_JOINT(tmpPallet.pointAnFrame[i], tmpPallet.pointAnPointPro[i],
                                              palletList[palletIndex].stackPointA_n[i].ufValue, palletList[palletIndex].stackPointA_n[i].utValue);
            //tmpPallet.pointAnPointPro[i].configString = tmpPallet.configString;
        }
         tmpPallet.pointAnPointPro[i].configString = tmpPallet.configString;
    }
    //tmpPallet.pointRnPointPro.resize(rnAmount);
    for (int i = 0; i < rnAmount; i++)
    {
        if (palletList[palletIndex].stackPointR_n[i].jointFlag == 1)
        {
            tmpPallet.pointRnPointPro[i] = coordinate->frameToPointPro_CART(tmpPallet.pointRnFrame[i],
                                                                                            palletList[palletIndex].stackPointR_n[i].ufValue,
                                                                                            palletList[palletIndex].stackPointR_n[i].utValue);
            //tmpPallet.pointRnPointPro[i].configString = tmpPallet.configString;
        }
        else
        {
            coordinate->frameToPointPro_JOINT(tmpPallet.pointRnFrame[i], tmpPallet.pointRnPointPro[i],
                                              palletList[palletIndex].stackPointR_n[i].ufValue, palletList[palletIndex].stackPointR_n[i].utValue);
//            tmpPallet.pointRnPointPro[i].configString = tmpPallet.configString;
        }
        tmpPallet.pointRnPointPro[i].configString = tmpPallet.configString;
    }
}

// 2017-3-25 彭秋明添加
PointPro PalletCalculate::cartToPointProJoint(PointPro pointPro)
{
    Frame f;
    PointPro pout = pointPro;
    f = coordinate->pointProToFrame(pointPro);
    coordinate->frameToPointPro_JOINT(f, pout, pointPro.ufValue, pointPro.utValue);
    return pout;
}

int PalletCalculate::initStackInfo(int palletIndex)
{
    if((palletAmount <= palletIndex)||(palletIndex < 0)){//超出范围
        return 0;
    }
    switch (palletList[palletIndex].stackType)
    {
        case ENUM_STACK_B:
        {
            // 2017-3-17 彭秋明修改，增加辅助点判断
            if (palletList[palletIndex].auxiliary == 1) //有辅助点
            {
                addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4007, 1);
                return -2;
                //示教点数量不对
//                if((5!=palletList[palletIndex].stackStylePointInfo.size())
//                            ||(5 != palletList[palletIndex].stackPointCount))
//                {
//                    //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4001, 1, palletList[palletIndex].stackPointCount);
//                    return -2;
//                 }
            }
            else
            {
                //示教点数量不对
                if ((4 == palletList[palletIndex].stackStylePointInfo.size()) && (4 == palletList[palletIndex].stackPointCount))
                {
                    if ((1 == palletList[palletIndex].row) || (1 == palletList[palletIndex].col) || (1 == palletList[palletIndex].layer))
                    {
                        return -2;
                    }
                    teachNumberType = ENUM_STACK_TEACHNUMBER_RCL;
                }
                else if ((3 == palletList[palletIndex].stackStylePointInfo.size()) && (3 == palletList[palletIndex].stackPointCount))
                {
                    if ((1 == palletList[palletIndex].layer) && (1 != palletList[palletIndex].row) && (1 != palletList[palletIndex].col))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_RC;
                    }
                    else if ((1 == palletList[palletIndex].col) && (1 != palletList[palletIndex].row) && (1 != palletList[palletIndex].layer))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_RL;
                    }
                    else if ((1 == palletList[palletIndex].row) && (1 != palletList[palletIndex].col) && (1 != palletList[palletIndex].layer))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_CL;
                    }
                    else
                    {
                        return -2;
                    }
                }
                else if ((2 == palletList[palletIndex].stackStylePointInfo.size()) && (2 == palletList[palletIndex].stackPointCount))
                {
                    if ((1 == palletList[palletIndex].layer) && (1 == palletList[palletIndex].col) && (1 != palletList[palletIndex].row))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_R;
                    }
                    else if ((1 == palletList[palletIndex].layer) && (1 == palletList[palletIndex].row) && (1 != palletList[palletIndex].col))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_C;
                    }
                    else if ((1 == palletList[palletIndex].row) && (1 == palletList[palletIndex].col) && (1 != palletList[palletIndex].layer))
                    {
                        teachNumberType = ENUM_STACK_TEACHNUMBER_L;
                    }
                    else
                    {
                        return -2;
                    }
                }
                else
                {
                    return -2;
                }
//                if((4!=palletList[palletIndex].stackStylePointInfo.size())
//                            ||(4 != palletList[palletIndex].stackPointCount))
//                {
//                    //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4001, 1, palletList[palletIndex].stackPointCount);
//                    return -2;
//                 }

                // 2017-3-17 彭秋明添加，判断示教间隔类型
                if(3 == palletList[palletIndex].intervalEnable.size())
                {
                    if (palletList[palletIndex].intervalEnable[0] == 1 && palletList[palletIndex].intervalEnable[1] == 1
                            && palletList[palletIndex].intervalEnable[2] == 1)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_RCL;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 1 && palletList[palletIndex].intervalEnable[1] == 1
                            && palletList[palletIndex].intervalEnable[2] == 0)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_RC;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 1 && palletList[palletIndex].intervalEnable[1] == 0
                            && palletList[palletIndex].intervalEnable[2] == 1)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_RL;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 0 && palletList[palletIndex].intervalEnable[1] == 1
                            && palletList[palletIndex].intervalEnable[2] == 1)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_CL;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 1 && palletList[palletIndex].intervalEnable[1] == 0
                            && palletList[palletIndex].intervalEnable[2] == 0)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_ROW;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 0 && palletList[palletIndex].intervalEnable[1] == 1
                            && palletList[palletIndex].intervalEnable[2] == 0)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_COL;
                    }
                    else if (palletList[palletIndex].intervalEnable[0] == 0 && palletList[palletIndex].intervalEnable[1] == 0
                            && palletList[palletIndex].intervalEnable[2] == 1)
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_LAYER;
                    }
                    else
                    {
                        teachIntervalType = ENUM_STACK_INTERVAL_NONE;
                    }
                }
                else
                {
                    teachIntervalType = ENUM_STACK_INTERVAL_NONE;
                }
            }
            if(initStackInfo_B(palletList[palletIndex], palletIndex)<0){
                return -3;
            }
            break;
        }
//        case ENUM_STACK_E:
//        {
//            break;
//        }
//        case ENUM_STACK_BX:
//        {
//            break;
//        }
//        case ENUM_STACK_EX:
//        {
//            break;
//        }
        default:
        {
            return -1;
        }
    }
    return 1;
}

int PalletCalculate::initStackInfo_B(struct StackInfo stack, int palletIndex)
{
    //---------求码垛行 列 层的方向偏移向量---------
    double eps = 1e-4;
    Matrix3d palletMatrix(3, 3);
    QVector<Vector> stackDeltaVector;
    stackDeltaVector.resize(3);
    std::vector<int> configIntTmp;
    std::string configStringTmp;
    //2017-3-25 彭秋明修改
    if (stack.stackStylePointInfo[0].stackPoint.jointFlag == 1)
    {
        std::vector<float> tmpDebug = cartToPointProJoint(stack.stackStylePointInfo[0].stackPoint).positionValue;
//        for(int i = 0 ;i <tmpDebug.size();i++ )
//        {
//            qDebug()<<i+1<<" pos:"<<tmpDebug[i];
//        }

        coordinate->getMultiConfig(tmpDebug,
                palletVector[palletIndex].configString, configIntTmp);//示教点多圈值


    }
    else
    {
        coordinate->getMultiConfig(stack.stackStylePointInfo[0].stackPoint.positionValue,
                palletVector[palletIndex].configString, configIntTmp);//示教点多圈值
    }
    //debug
    // 2017-4-6 调试
    if(IF_DEBUG)
    {
        qDebug() << "-------palletVector[palletIndex].configString------" << QString::fromStdString(palletVector[palletIndex].configString) << "\n\n\n";
        qDebug() << "-------********************.configString------" << QString::fromStdString(stack.stackStylePointInfo[0].stackPoint.configString) << "\n\n\n";
    }

    Frame tmpFrame=coordinate->pointProToFrame( stack.stackStylePointInfo[0].stackPoint );
    Vector stackVector0 = tmpFrame.p;
    if(IF_DEBUG)
    {
        //debug
        qDebug() << "---stackPointCount----" << stack.stackPointCount << "------stackStylePointInfo.size()------" << stack.stackStylePointInfo.size() << "------------";
    }

    for(int i =1; i<stack.stackStylePointInfo.size(); i++)
    {

        //2017-3-25 彭秋明修改
        if (stack.stackStylePointInfo[i].stackPoint.jointFlag == 1)
        {
            coordinate->getMultiConfig(cartToPointProJoint(stack.stackStylePointInfo[i].stackPoint).positionValue,
                                       configStringTmp, configIntTmp);//示教点多圈值
        }
        else
        {
            coordinate->getMultiConfig(stack.stackStylePointInfo[i].stackPoint.positionValue,
                    configStringTmp, configIntTmp);//示教点多圈值
        }
        if(palletVector[palletIndex].configString != configStringTmp)
        {
            if(IF_DEBUG)
            {
                qDebug()<<"PalletCalculate::initStackInfo_B====第"<< i <<"个示教点多解多圈与原点的不一致"<<
                          QString::fromStdString(palletVector[palletIndex].configString)<<QString::fromStdString(configStringTmp);
            }
            //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4002, 1, 0, "示教点");
            return -1;
        }
        tmpFrame=coordinate->pointProToFrame(stack.stackStylePointInfo[i].stackPoint);
        palletMatrix(0, i-1) = (stack.stackStylePointInfo[i]).rowColumnLayer[0] - (stack.stackStylePointInfo[0]).rowColumnLayer[0];
        palletMatrix(1, i-1) = (stack.stackStylePointInfo[i]).rowColumnLayer[1] - (stack.stackStylePointInfo[0]).rowColumnLayer[1];
        palletMatrix(2, i-1) = (stack.stackStylePointInfo[i]).rowColumnLayer[2] - (stack.stackStylePointInfo[0]).rowColumnLayer[2];
        stackDeltaVector[i-1] = tmpFrame.p - stackVector0;
    }
//    if(fabs(palletMatrix.determinant()) < eps){//四个点在同一平面上，无法计算
//        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4003, 1);
//        return -2;
//    }
    //2017-4-18 彭秋明添加
    //缺少的行、列、层示教点对应的偏移量置0,矩阵palletMatrix则在缺少的位置置1,保证为对角阵，方便计算
    switch (teachNumberType)
    {
        case ENUM_STACK_TEACHNUMBER_RCL:
        {
            break;
        }
        case ENUM_STACK_TEACHNUMBER_RC:
        {
            for (int i = 0; i < 2; i++)
            {
                palletMatrix(i, 2) = 0;
            }
            palletMatrix(2, 2) = 1;
            stackDeltaVector[2] = stackVector0 - stackVector0;
            break;
        }
        case ENUM_STACK_TEACHNUMBER_RL:
        {
            for (int i = 0; i < 3; i++)
            {
                palletMatrix(i, 2) = palletMatrix(i, 1);
                palletMatrix(i, 1) = 0;
            }
            palletMatrix(1, 1) = 1;
            stackDeltaVector[2] = stackDeltaVector[1];
            stackDeltaVector[1] = stackVector0 - stackVector0;
            break;
        }
        case ENUM_STACK_TEACHNUMBER_CL:
        {
            for (int i = 0; i < 3; i++)
            {
                palletMatrix(i, 2) = palletMatrix(i, 1);
                palletMatrix(i, 1) = palletMatrix(i, 0);
                palletMatrix(i, 0) = 0;
            }
            palletMatrix(0, 0) = 1;
            stackDeltaVector[2] = stackDeltaVector[1];
            stackDeltaVector[1] = stackDeltaVector[0];
            stackDeltaVector[0] = stackVector0 - stackVector0;
            break;
        }
        case ENUM_STACK_TEACHNUMBER_R:
        {
            for (int i = 0; i < 3; i++)
            {
                palletMatrix(i, 2) = 0;
                palletMatrix(i, 1) = 0;
            }
            palletMatrix(1, 1) = 1;
            palletMatrix(2, 2) = 1;
            stackDeltaVector[2] = stackVector0 - stackVector0;
            stackDeltaVector[1] = stackVector0 - stackVector0;
            break;
        }
        case ENUM_STACK_TEACHNUMBER_C:
        {
            for (int i = 0; i < 3; i++)
            {
                palletMatrix(i, 2) = 0;
                palletMatrix(i, 1) = palletMatrix(i, 0);
                palletMatrix(i, 0) = 0;
            }
            palletMatrix(0, 0) = 1;
            palletMatrix(2, 2) = 1;
            stackDeltaVector[2] = stackVector0 - stackVector0;
            stackDeltaVector[1] = stackDeltaVector[0];
            stackDeltaVector[0] = stackVector0 - stackVector0;
            break;
        }
        case ENUM_STACK_TEACHNUMBER_L:
        {
            for (int i = 0; i < 3; i++)
            {
                palletMatrix(i, 2) = palletMatrix(i, 0);
                palletMatrix(i, 1) = 0;
                palletMatrix(i, 0) = 0;
            }
            palletMatrix(0, 0) = 1;
            palletMatrix(1, 1) = 1;
            stackDeltaVector[2] = stackDeltaVector[0];
            stackDeltaVector[1] = stackVector0 - stackVector0;
            stackDeltaVector[0] = stackVector0 - stackVector0;
            break;
        }
        default:
        {
            return -3;
        }
    }
    // 2017-3-17 彭秋明修改
    switch (teachIntervalType)
    {
        case ENUM_STACK_INTERVAL_ROW:
        {
            palletMatrix(0, 0) = stack.row - 1;
            double rowLength = (stack.row - 1) * stack.intervalValue[0]; //码垛行方向的总长度
            double alpha = atan2(stackDeltaVector[0][1], stackDeltaVector[0][0]); //码垛行方向与世界坐标x轴的夹角
            stackDeltaVector[0][0] = rowLength * cos(alpha) / 1000.0; //码垛行端点的x坐标
            stackDeltaVector[0][1] = rowLength * sin(alpha) / 1000.0; //码垛行端点的y坐标
            break;
        }
        case ENUM_STACK_INTERVAL_COL:
        {
            palletMatrix(1, 1) = stack.col - 1;
            double colLength = (stack.col - 1) * stack.intervalValue[1]; //码垛列方向的总长度
            double beta = atan2(stackDeltaVector[1][0], stackDeltaVector[1][1]); //码垛列方向与世界坐标y轴的夹角
            stackDeltaVector[1][0] = colLength * sin(beta) / 1000.0; //码垛列端点的x坐标
            stackDeltaVector[1][1] = colLength * cos(beta) / 1000.0; //码垛列端点的y坐标
            break;
        }
        case ENUM_STACK_INTERVAL_LAYER:
        {
            palletMatrix(2, 2) = stack.layer - 1;
            double layerLength = (stack.layer - 1) * stack.intervalValue[2]; //码垛层方向的总长度
            stackDeltaVector[2][2] = layerLength;
            break;
        }
        case ENUM_STACK_INTERVAL_RC:
        {
            palletMatrix(0, 0) = stack.row - 1;
            palletMatrix(1, 1) = stack.col - 1;
            double rowLength = (stack.row - 1) * stack.intervalValue[0]; //码垛行方向的总长度
            double colLength = (stack.col - 1) * stack.intervalValue[1]; //码垛列方向的总长度
            double alpha = atan2(stackDeltaVector[0][1], stackDeltaVector[0][0]); //码垛行方向与世界坐标x轴的夹角
            double beta = atan2(stackDeltaVector[1][0], stackDeltaVector[1][1]); //码垛列方向与世界坐标y轴的夹角
            stackDeltaVector[0][0] = rowLength * cos(alpha) / 1000.0; //码垛行端点的x坐标
            stackDeltaVector[0][1] = rowLength * sin(alpha) / 1000.0; //码垛行端点的y坐标
            stackDeltaVector[1][0] = colLength * sin(beta) / 1000.0; //码垛列端点的x坐标
            stackDeltaVector[1][1] = colLength * cos(beta) / 1000.0; //码垛列端点的y坐标
            break;
        }
        case ENUM_STACK_INTERVAL_RL:
        {
            palletMatrix(0, 0) = stack.row - 1;
            palletMatrix(2, 2) = stack.layer - 1;
            double rowLength = (stack.row - 1) * stack.intervalValue[0]; //码垛行方向的总长度
            double layerLength = (stack.layer - 1) * stack.intervalValue[2]; //码垛层方向的总长度
            double alpha = atan2(stackDeltaVector[0][1], stackDeltaVector[0][0]); //码垛行方向与世界坐标x轴的夹角
            stackDeltaVector[0][0] = rowLength * cos(alpha) / 1000.0; //码垛行端点的x坐标
            stackDeltaVector[0][1] = rowLength * sin(alpha) / 1000.0; //码垛行端点的y坐标
            stackDeltaVector[2][2] = layerLength;
            break;
        }
        case ENUM_STACK_INTERVAL_CL:
        {
            palletMatrix(1, 1) = stack.col - 1;
            palletMatrix(2, 2) = stack.layer - 1;
            double colLength = (stack.col - 1) * stack.intervalValue[1]; //码垛列方向的总长度
            double layerLength = (stack.layer - 1) * stack.intervalValue[2]; //码垛层方向的总长度
            double beta = atan2(stackDeltaVector[1][0], stackDeltaVector[1][1]); //码垛列方向与世界坐标y轴的夹角
            stackDeltaVector[1][0] = colLength * sin(beta) / 1000.0; //码垛列端点的x坐标
            stackDeltaVector[1][1] = colLength * cos(beta) / 1000.0; //码垛列端点的y坐标
            stackDeltaVector[2][2] = layerLength;
            break;
        }
        case ENUM_STACK_INTERVAL_RCL:
        {
            palletMatrix(0, 0) = stack.row - 1;
            palletMatrix(1, 1) = stack.col - 1;
            palletMatrix(2, 2) = stack.layer - 1;
            double rowLength = (stack.row - 1) * stack.intervalValue[0]; //码垛行方向的总长度
            double colLength = (stack.col - 1) * stack.intervalValue[1]; //码垛列方向的总长度
            double layerLength = (stack.layer - 1) * stack.intervalValue[2]; //码垛层方向的总长度
            double alpha = atan2(stackDeltaVector[0][1], stackDeltaVector[0][0]); //码垛行方向与世界坐标x轴的夹角
            double beta = atan2(stackDeltaVector[1][0], stackDeltaVector[1][1]); //码垛列方向与世界坐标y轴的夹角
            stackDeltaVector[0][0] = rowLength * cos(alpha) / 1000.0; //码垛行端点的x坐标
            stackDeltaVector[0][1] = rowLength * sin(alpha) / 1000.0; //码垛行端点的y坐标
            stackDeltaVector[1][0] = colLength * sin(beta) / 1000.0; //码垛列端点的x坐标
            stackDeltaVector[1][1] = colLength * cos(beta) / 1000.0; //码垛列端点的y坐标
            stackDeltaVector[2][2] = layerLength;
            break;
        }
        default:
            break;
    }
    //2017-3-16 彭秋明修改，顶层示教时，应把顶层的两个示教点layer方向的偏移置为０。再计算整体行列层方向的单位偏移
    if (palletList[palletIndex].teachOrder == 0) //顶层示教
    {
        palletMatrix(2, 0) = 0;
        palletMatrix(2, 1) = 0;

        stackDeltaVector[0][2] = 0;
        stackDeltaVector[1][2] = 0;
    }
    Matrix3d palletInverse = palletMatrix.inverse();
    palletVector[palletIndex].xAxisVector = palletInverse(0,0) * stackDeltaVector[0] +
                                                                        palletInverse(1,0) * stackDeltaVector[1] +
                                                                        palletInverse(2,0) * stackDeltaVector[2];
    palletVector[palletIndex].yAxisVector = palletInverse(0,1) * stackDeltaVector[0] +
                                                                        palletInverse(1,1) * stackDeltaVector[1] +
                                                                        palletInverse(2,1) * stackDeltaVector[2];
    palletVector[palletIndex].zAxisVector = palletInverse(0,2) * stackDeltaVector[0] +
                                                                        palletInverse(1,2) * stackDeltaVector[1] +
                                                                        palletInverse(2,2) * stackDeltaVector[2];

    //---------计算码垛原点的接近点/逃离点/堆叠点---------
    //计算码垛原点接近点frame
    int anAmount = palletList[palletIndex].stackProximityCount;//接近点数目
    palletVector[palletIndex].pointAnFrame.resize(anAmount);
    // 2017-3-27 彭秋明添加
    palletVector[palletIndex].pointAnPointPro.resize(anAmount);

    for(int j=0; j<anAmount; j++)
    {
        //coordinate->getMultiConfig(stack.stackPointA_n[j].positionValue, configStringTmp, configIntTmp);//接近点多圈值
        //2017-3-25 彭秋明修改
        if (stack.stackPointA_n[j].jointFlag == 1)
        {
            coordinate->getMultiConfig(cartToPointProJoint(stack.stackPointA_n[j]).positionValue,
                                       configStringTmp, configIntTmp);//接近点多圈值
        }
        else
        {
            coordinate->getMultiConfig(stack.stackPointA_n[j].positionValue, configStringTmp, configIntTmp);//接近点多圈值
        }

        if(palletVector[palletIndex].configString != configStringTmp)
        {
            if(IF_DEBUG)
            {
                qDebug()<<"PalletCalculate::initStackInfo_B====第"<< j <<"个接近点多解多圈与原点的不一致"<<
                          QString::fromStdString(palletVector[palletIndex].configString)<<QString::fromStdString(configStringTmp);
            }
            //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4002, 1, 0, "接近点");
            return -1;
        }
        palletVector[palletIndex].pointAnFrame[j] = coordinate->pointProToFrame( stack.stackPointA_n[j] );
    }

    //计算码垛原点逃离点frame
    int rnAmount = palletList[palletIndex].stackFleeCount;//逃离点数目
    palletVector[palletIndex].pointRnFrame.resize(rnAmount);
    // 2017-3-27 彭秋明添加
    palletVector[palletIndex].pointRnPointPro.resize(rnAmount);

    for(int j=0; j<rnAmount; j++)
    {
        //coordinate->getMultiConfig(stack.stackPointR_n[j].positionValue, configStringTmp, configIntTmp);//逃离点多圈值
        //2017-3-25 彭秋明修改
        if (stack.stackPointR_n[j].jointFlag == 1)
        {
            coordinate->getMultiConfig(cartToPointProJoint(stack.stackPointR_n[j]).positionValue,
                                       configStringTmp, configIntTmp);//逃离点多圈值
        }
        else
        {
            coordinate->getMultiConfig(stack.stackPointR_n[j].positionValue, configStringTmp, configIntTmp);//逃离点多圈值
        }

        if(palletVector[palletIndex].configString != configStringTmp)
        {
//            if(IF_DEBUG)
//            {
//                qDebug()<<"PalletCalculate::initStackInfo_B====第"<< j <<"个逃离点多解多圈与原点的不一致"<<
//                          QString::fromStdString(palletVector[palletIndex].configString)<<QString::fromStdString(configStringTmp);
//            }
            //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4002, 1, 0, "逃离点");
            return -1;
        }
        palletVector[palletIndex].pointRnFrame[j] = coordinate->pointProToFrame( stack.stackPointR_n[j] );
    }

    //计算码垛原点堆叠点frame
    //coordinate->getMultiConfig(stack.stackPointBTM.positionValue, configStringTmp, configIntTmp);//堆叠点多圈值
    //2017-3-25 彭秋明修改
    if (stack.stackPointBTM.jointFlag == 1)
    {
        coordinate->getMultiConfig(cartToPointProJoint(stack.stackPointBTM).positionValue,
                                   configStringTmp, configIntTmp);//堆叠点多圈值
    }
    else
    {
        coordinate->getMultiConfig(stack.stackPointBTM.positionValue, configStringTmp, configIntTmp);//堆叠点多圈值
    }

    if(palletVector[palletIndex].configString != configStringTmp)
    {
//        if(IF_DEBUG)
//        {
//            qDebug()<<"PalletCalculate::initStackInfo_B====堆叠点多解多圈与原点的不一致"<<
//                      QString::fromStdString(palletVector[palletIndex].configString)<<QString::fromStdString(configStringTmp);
//        }
        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4002, 1, 0, "堆叠点");
        return -1;
    }
    palletVector[palletIndex].btmFrame = coordinate->pointProToFrame( stack.stackPointBTM );
    // 2017-3-20 彭秋明添加 2017-3-25修改
    frameToPointPro(palletIndex, anAmount, rnAmount, palletVector[palletIndex]);
    //-----------码垛是否完成 标识位初始化------------------
    palletVector[palletIndex].finishOutput = 2; //未初始化时为０，初始化后码垛完成为１，初始化后未完成码垛为２
    palletVector[palletIndex].firstNum = 0;
    qDebug()<<"*** *** ***"<<__FUNCTION__<<__LINE__<<"palletVector[palletIndex].firstNum = 0";

    //---------码垛行 列 层初始化---------
//    palletVector[palletIndex].row = 0;
//    palletVector[palletIndex].col = 0;
//    palletVector[palletIndex].layer = 0;
    if(palletList[palletIndex].stackUpDown == ENUM_STACK_DOWN)//拆垛
    {
        // 2017-3-15 彭秋明修改，初始行列层需根据加法运算和减法运算不同而设定
        if (1 == palletList[palletIndex].execIncr ) //加法运算时初始行列层
        {
            palletVector[palletIndex].row = stack.row;
            palletVector[palletIndex].col = stack.col;
            palletVector[palletIndex].layer = stack.layer;
        }
        else //减法运算时初始行列层
        {
            palletVector[palletIndex].row = 1;
            palletVector[palletIndex].col = 1;
            palletVector[palletIndex].layer = palletList[palletIndex].layer;
        }
    }
    else//码垛
    {
        // 2017-3-15 彭秋明修改，初始行列层需根据加法运算和减法运算不同而设定
        if (1 == palletList[palletIndex].execIncr) //加法运算时初始行列层
        {
            palletVector[palletIndex].row = 1;
            palletVector[palletIndex].col = 1;
            palletVector[palletIndex].layer = 1;
        }
        else //减法运算时初始行列层
        {
            palletVector[palletIndex].row = stack.row;
            palletVector[palletIndex].col = stack.col;
            palletVector[palletIndex].layer = 1;
        }
    }
    //2017-4-22 彭秋明添加，码垛初始位置行列层
    palletRCL[palletIndex].row = palletVector[palletIndex].row;
    palletRCL[palletIndex].col = palletVector[palletIndex].col;
    palletRCL[palletIndex].layer = palletVector[palletIndex].layer;
    return 1;
}

int PalletCalculate::calculatePallet(int palletIndex, int row, int col, int layer)
{
    if( palletIndex >= palletAmount || 0 > palletIndex){//超出码垛范围
        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4004, 1);
        return -1;
    }
    if(( row > palletList[palletIndex].row )||(1 > row) ||
        (col > palletList[palletIndex].col)||(1 > col) ||
        (layer > palletList[palletIndex].layer) ||(1 > layer)){//行列层超出范围
        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4005, 1);
        return -2;
    }

    int anAmount = palletVector[palletIndex].pointAnFrame.size();//接近点数量
    int rnAmount = palletVector[palletIndex].pointRnFrame.size();//逃离点数量

    if(row != palletVector[palletIndex].row)//行变化
    {
        Vector deltaRow = (row-palletVector[palletIndex].row) * palletVector[palletIndex].xAxisVector;
        palletVector[palletIndex].row = row;
        //debug
//        for (int i = 0; i < 3; i++)
//        {
//            qDebug() << "----deltaRow.data----" << deltaRow.data[i] << "---------------";
//        }
        palletVector[palletIndex].btmFrame.p = palletVector[palletIndex].btmFrame.p + deltaRow;
        for(int i=0; i<anAmount; i++){
            palletVector[palletIndex].pointAnFrame[i].p = palletVector[palletIndex].pointAnFrame[i].p + deltaRow;
        }
        for(int i=0; i<rnAmount; i++){
            palletVector[palletIndex].pointRnFrame[i].p = palletVector[palletIndex].pointRnFrame[i].p + deltaRow;
        }
    }

    if(col != palletVector[palletIndex].col)//列变化
    {
        Vector deltaCol = (col-palletVector[palletIndex].col) * palletVector[palletIndex].yAxisVector;
        palletVector[palletIndex].col = col;
        palletVector[palletIndex].btmFrame.p = palletVector[palletIndex].btmFrame.p + deltaCol;
        for(int i=0; i<anAmount; i++){
            palletVector[palletIndex].pointAnFrame[i].p = palletVector[palletIndex].pointAnFrame[i].p + deltaCol;
        }
        for(int i=0; i<rnAmount; i++){
            palletVector[palletIndex].pointRnFrame[i].p = palletVector[palletIndex].pointRnFrame[i].p + deltaCol;
        }
    }

    if(layer != palletVector[palletIndex].layer)//层变化
    {
        Vector deltaLayer = (layer-palletVector[palletIndex].layer) * palletVector[palletIndex].zAxisVector;
        palletVector[palletIndex].layer = layer;
        palletVector[palletIndex].btmFrame.p = palletVector[palletIndex].btmFrame.p + deltaLayer;
        for(int i=0; i<anAmount; i++){
            palletVector[palletIndex].pointAnFrame[i].p = palletVector[palletIndex].pointAnFrame[i].p + deltaLayer;
        }
        for(int i=0; i<rnAmount; i++){
            palletVector[palletIndex].pointRnFrame[i].p = palletVector[palletIndex].pointRnFrame[i].p + deltaLayer;
        }
    }
    // 2017-3-20 彭秋明添加 2017-3-25修改
    frameToPointPro(palletIndex, anAmount, rnAmount, palletVector[palletIndex]);

    return 1;
}

int PalletCalculate::calculateNext(int palletIndex)
{
    if(palletIndex >= palletAmount)//码垛号超过范围
    {
        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4004, 1);
        return -1;
    }

//    row = palletVector[palletIndex].row;
//    col = palletVector[palletIndex].col;
//    layer = palletVector[palletIndex].layer;
    // 2017-3-25 彭秋明修改   if(IF_DEBUG)
    if(IF_DEBUG)
    {
    qDebug() << "-----------firstNum1-------------" << palletVector[palletIndex].firstNum << "/n/n/n/n" ;
    }
    if (0 == palletVector[palletIndex].firstNum) //先运行第一个码垛点再计算判断下一个
    {
        row = palletRCL[palletIndex].row;
        col = palletRCL[palletIndex].col;
        layer = palletRCL[palletIndex].layer;
        palletVector[palletIndex].firstNum = 1;
    }
    else if (1 == palletVector[palletIndex].modifyPlValue) //修改PL后，先运行PL行列层的点再计算下一个
    {
        row = palletPlRCL[palletIndex].row;
        col = palletPlRCL[palletIndex].col;
        layer = palletPlRCL[palletIndex].layer;
        palletVector[palletIndex].modifyPlValue = 0;
    }
    else
    {
        row = palletVector[palletIndex].row;
        col = palletVector[palletIndex].col;
        layer = palletVector[palletIndex].layer;
        getNextPalletInfo(palletIndex);
    }

   // qDebug() << "-----------firstNum2-------------" << palletVector[palletIndex].firstNum << "/n/n/n/n" ;

    calculatePallet(palletIndex, row, col, layer);
   //qDebug() << "-----------firstNum3-------------" << palletVector[palletIndex].firstNum << "/n/n/n/n" ;
    //彭秋明添加
    if (1 == palletVector[palletIndex].finishOutput)
    {
        return 2;
    }

    number++;
   // qDebug() << "-----码垛点数：------" << number << "---------------/b/b/b/";
    //qDebug() << "------------" << palletVector[palletIndex].row << palletVector[palletIndex].col << palletVector[palletIndex].layer << "---//n/n/n/n/";
    return 1;
}

// 2017-3-31 彭秋明添加修改
void PalletCalculate::getNextPalletInfo(int palletIndex)
{
    if(palletList[palletIndex].stackUpDown == ENUM_STACK_UP)//码垛
    {
        if(palletList[palletIndex].execOrder == ENUM_STACK_ORDER_RCL)//执行顺序是行列层
        {
            if(IF_DEBUG)
            {
            qDebug() << "----------------行列层：" << row << "," << col << "," << layer << "----------------------";
            }
            if(1==palletList[palletIndex].execIncr)//增加
            {
                if(row < palletList[palletIndex].row)//行没结束
                {
                    row++;//下一行
                }
                else if(col < palletList[palletIndex].col)//列没结束
                {
                    row = 1;
                    col++;
                }
                else if(layer < palletList[palletIndex].layer)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer++;
                }
                else//码垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;  //码垛完成信号
                  //  std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
                //2017-4-14
                if (row == palletList[palletIndex].row && col == palletList[palletIndex].col && layer == palletList[palletIndex].layer)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
              //      std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }

            }//end if(1==pointData->basePalletList[palletIndex].execIncr)//增加
            else if(-1== palletList[palletIndex].execIncr) //减少
            {
                if(row > 1)//行没结束
                {
                    row--;
                }
                else if(col > 1)//列没结束
                {
                    row = palletList[palletIndex].row ;
                    col--;
                }
                else if(layer < palletList[palletIndex].layer)//层没结束
                {
                    row = palletList[palletIndex].row;
                    col = palletList[palletIndex].col;
                    layer ++;
                }
                else //最后一层，码垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == palletList[palletIndex].layer)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
               //     std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }//end else//减少
            else
            {

            }
        }//end if( 0==pointData->basePalletList[palletIndex].execOrder )//执行顺序是行列层
        else if(palletList[palletIndex].execOrder == ENUM_STACK_ORDER_CRL) //列行层
        {
            if(1==palletList[palletIndex].execIncr)//增加
            {
                if(col < palletList[palletIndex].col)//列没结束
                {
                    col++;
                }
                else if(row < palletList[palletIndex].row)//行没结束
                {
                    row++;
                    col = 1;
                }
                else if(layer < palletList[palletIndex].layer)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer++;
                }
                else//最后一层，码垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }

                //2017-4-14
                if (row == palletList[palletIndex].row && col < palletList[palletIndex].col && layer == palletList[palletIndex].layer)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
            }
            else if(-1== palletList[palletIndex].execIncr) //减少
            {
                if(col > 1)//列没结束
                {
                    col--;
                }
                else if(row > 1)//行没结束
                {
                    row--;
                    col = palletList[palletIndex].col;
                }
                else if(layer < palletList[palletIndex].layer)//层没结束
                {
                    row = palletList[palletIndex].row;
                    col = palletList[palletIndex].col;
                    layer++;
                }
                else//最后一层，码垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == palletList[palletIndex].layer)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
              //      std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }//end else//减少
            else
            {

            }
        }//end else//列行层
    }//end  if(0==pointData->basePalletList[palletIndex].stackUpDown)//码垛
    else if (palletList[palletIndex].stackUpDown == ENUM_STACK_DOWN) //拆垛
    {
        if(palletList[palletIndex].execOrder == ENUM_STACK_ORDER_RCL)//执行顺序是行列层
        {
            if(1==palletList[palletIndex].execIncr)//增加
            {
                if(row > 1)//行没结束
                {
                    row--;
                }
                else if(col > 1)//列没结束
                {
                    row = palletList[palletIndex].row;
                    col--;
                }
                else if(layer > 1)//层没结束
                {
                    row = palletList[palletIndex].row;
                    col = palletList[palletIndex].col;
                    layer--;
                }
                else//第一层，拆垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == 1)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
            //        std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }//end if(1==pointData->basePalletList[palletIndex].execIncr)//增加
            else if(-1 == palletList[palletIndex].execIncr) //减少
            {
                if(row < palletList[palletIndex].row)//行没结束
                {
                    row++;
                }
                else if(col < palletList[palletIndex].col)//列没结束
                {
                    row = 1;
                    col++;
                }
                else if(layer > 1)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer--;
                }
                else //第一层，拆垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == palletList[palletIndex].row && col == palletList[palletIndex].col && layer == 1)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
        //            std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }//end else//减少
        }//end if( 0==pointData->basePalletList[palletIndex].execOrder )//执行顺序是行列层
        else if(palletList[palletIndex].execOrder == ENUM_STACK_ORDER_CRL) //列行层
        {
            if(1==palletList[palletIndex].execIncr)//增加
            {
                if(col > 1 )//列没结束
                {
                    col--;
                }
                else if(row > 1)//行没结束
                {
                    row--;
                    col = palletList[palletIndex].col;
                }
                else if(layer > 1)//层没结束
                {
                    row = palletList[palletIndex].row;
                    col = palletList[palletIndex].col;
                    layer--;
                }
                else//最后一层，码垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == 1)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
       //             std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }
            else if(-1==palletList[palletIndex].execIncr)//减少
            {
                if(col < palletList[palletIndex].col)//列没结束
                {
                    col++;
                }
                else if(row < palletList[palletIndex].row)//行没结束
                {
                    row++;
                    col = 1;
                }
                else if(layer > 1)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer--;
                }
                else //最后一层，拆垛完成
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
                    return;
                }
                //2017-4-14
                if (row == palletList[palletIndex].row && col == palletList[palletIndex].col && layer == 1)
                {
                    palletVector[palletIndex].finishOutput = ENUM_STACK_WORK_FINISH;
         //           std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return;
                }
            }//end else//减少
        }//end else//列行层
    }//end else//拆垛
}

// 2017-3-22 彭秋明修改
int PalletCalculate::getPointData(struct PalletPoint palletPoint, struct PointPro &p)
{
    qDebug() << "222-----------码垛号：" << palletPoint.palletNumber << "-----------" << "------------码垛数量：" << palletAmount << "-------------";
    if((palletPoint.palletNumber < 0)||(palletPoint.palletNumber >= palletAmount)){
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4004, 1);
        return -1;//码垛号超过范围
    }

    switch (palletPoint.pointType)
    {
        case ENUM_STACK_POINT_BTM://堆叠点
        {
              p = palletVector[palletPoint.palletNumber].btmPointPro;
//            // 2017-3-24 调试
//            for (int i = 0; i < palletList[palletPoint.palletNumber].stackPointBTM.positionValue.size(); i++)
//            {
//                qDebug() << "jointFlag" << palletList[palletPoint.palletNumber].stackPointBTM.jointFlag <<
//                            palletList[palletPoint.palletNumber].stackPointBTM.positionValue[i];
//            }
            for (int i = 0; i < palletVector[palletPoint.palletNumber].btmPointPro.positionValue.size(); i++)
            {
                qDebug() <<  "pallet_btm_data:" << palletVector[palletPoint.palletNumber].btmPointPro.positionValue[i] << "-------";
            }
//            for (int i = 0; i < p.positionValue.size(); i++)
//            {
//                qDebug() << "palletToPoint" << p.positionValue[i];
//            }
            return 1;
        }
        case ENUM_STACK_POINT_AN://接近点
        {
            qDebug() << "-------接近点号：" << palletPoint.pointNumber << "------接近点数量：" << palletVector[palletPoint.palletNumber].pointAnFrame.size();
            if((palletPoint.pointNumber < 0)||palletPoint.pointNumber >= palletVector[palletPoint.palletNumber].pointAnFrame.size()){
                addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4006, 1, 0, "接近点号");
                return -2;//逃离点好超过范围
            }
            p = palletVector[palletPoint.palletNumber].pointAnPointPro[palletPoint.pointNumber];
            // 2017-3-24 调试
            for (int i = 0; i < palletVector[palletPoint.palletNumber].pointAnPointPro[palletPoint.pointNumber].positionValue.size(); i++)
            {
                qDebug() << "pallet_an_data:" << palletVector[palletPoint.palletNumber].pointAnPointPro[palletPoint.pointNumber].positionValue[i] << "------";
            }
//            for (int i = 0; i < p.positionValue.size(); i++)
//            {
//                qDebug() << "palletToPoint" << p.positionValue[i];
//            }
            return 1;
        }
        case ENUM_STACK_POINT_RN://逃离点
        {
            qDebug() << "-------逃离点号：" << palletPoint.pointNumber << "------逃离点数量：" << palletVector[palletPoint.palletNumber].pointRnFrame.size();
            if((palletPoint.pointNumber < 0)||palletPoint.pointNumber >= palletVector[palletPoint.palletNumber].pointRnFrame.size()){
                addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4001, 1, 0, "逃离点号");
                return -3;//接近点号超过范围
            }
            p = palletVector[palletPoint.palletNumber].pointRnPointPro[palletPoint.pointNumber];
            for (int i = 0; i < palletVector[palletPoint.palletNumber].pointRnPointPro[palletPoint.pointNumber].positionValue.size(); i++)
            {
                qDebug() << "pallet_rn_data:" << palletVector[palletPoint.palletNumber].pointRnPointPro[palletPoint.pointNumber].positionValue[i] << "------";
            }
            return 1;
        }
        default:
        {
            return -4;//不支持点类型
        }
    }
}

void PalletCalculate::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int _robotId,
                                     int parameter1, string parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;

    switch(messageCode)
    {
        case 4001://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   QObject::tr(")示教点数量不对：").toStdString() +
                   QString::number(parameter1).toStdString() +
                   QObject::tr("个示教点").toStdString();
           infomationStr = str1;
           break;
        }
        case 4002://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   parameter2 +
                   QObject::tr("多解多圈与原点不一致").toStdString();
           infomationStr = str1;
           break;
        }
        case 4003://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   QObject::tr("4个示教点在同一平面，不能计算").toStdString();
           infomationStr = str1;
           break;
        }
        case 4004://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   QObject::tr("码垛号超出码垛数量范围").toStdString();
           infomationStr = str1;
           break;
        }
        case 4005://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   QObject::tr("行列层号超出数量范围").toStdString();
           infomationStr = str1;
           break;
        }
        case 4006://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   parameter2 +
                   QObject::tr("超出范围").toStdString();
           infomationStr = str1;
           break;
        }
        case 4007://ENUM_MSG_ERROR
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(_robotId).toStdString() +
                   QObject::tr("暂不支持辅助点示教").toStdString();
           infomationStr = str1;
           break;
        }
        default:
        {
            qDebug()<<"PalletCalculate::addMsg====not match error code";
            break;
        }
    }

tmpMsg.MessageInformation = infomationStr;
MessageLog::getInstance()->addMessage(tmpMsg);
}

